/**
 * Copyright 2010 (c) Jack Pien
 */

#ifndef __DD_X_MSG_H
#define __DD_X_MSG_H

#ifndef DD_BUILD_PLATFORM
#ifndef DD_BUILD_BRAIN
#error "Need to define building brain or platform"
#endif
#endif

#define DD_PKT_MSG_OFFSET(buf) &(((uint8_t*)(buf))[DD_PACKET_BYTE_OFFSET_MSG])

// ----
// ---- Output String ----
// ----
#define DD_MSG_OUTPUT_STRING 2
typedef struct DDmsgOutputString_t {
    uint8_t reason;
    uint8_t nChars;
    uint8_t* charBuf;
} DDmsgOutputString;
uint32_t msgPackOutputString( const DDmsgOutputString* msg, uint8_t* buf ) {
    int idx=0;
    int i;
    buf[idx++] = DD_MSG_OUTPUT_STRING;
    buf[idx++] = msg->reason;
    buf[idx++] = msg->nChars;
    for( i=0; i < msg->nChars; i++ ) {
        buf[idx++] = msg->charBuf[i];
    }
    return idx;
}
void msgUnpackOutputString( uint8_t* buf, DDmsgOutputString* msg ) {
    int idx=1;
    msg->reason = buf[idx++];
    msg->nChars = buf[idx++];
    msg->charBuf = &(buf[idx]);
}

// ----
// ---- Direct Drive ----
// ----
#define DD_MSG_DIRECT_DRIVE 145
typedef struct DDmsgDirectDrive_t {
    int16_t rwvelocity;
    int16_t lwvelocity;
} DDmsgDirectDrive;
uint32_t msgPackDirectDrive( const DDmsgDirectDrive* msg, uint8_t* buf ) {
    int idx=0;
    buf[idx++] = DD_MSG_DIRECT_DRIVE;
    buf[idx++] = ((uint8_t*)(&msg->rwvelocity))[0];
    buf[idx++] = ((uint8_t*)(&msg->rwvelocity))[1];
    buf[idx++] = ((uint8_t*)(&msg->lwvelocity))[0];
    buf[idx++] = ((uint8_t*)(&msg->lwvelocity))[1];
    return idx;
}
void msgUnpackDirectDrive( uint8_t* buf, DDmsgDirectDrive* msg ) {
    int idx=1;
    ((uint8_t*)(&msg->rwvelocity))[0] = buf[idx++];
    ((uint8_t*)(&msg->rwvelocity))[1] = buf[idx++];
    ((uint8_t*)(&msg->lwvelocity))[0] = buf[idx++];
    ((uint8_t*)(&msg->lwvelocity))[1] = buf[idx++];
}
    
// ----
// ---- Ack ----
// ----
#define DD_MSG_ACK 255
typedef struct DDmsgAck_t {
    uint32_t ackTs;
} DDmsgAck;
uint32_t msgPackAck( const DDmsgAck* msg, uint8_t* buf ) {
    int idx=0;
    buf[idx++] = DD_MSG_ACK;
    buf[idx++] = ((uint8_t*)(&(msg->ackTs)))[0];
    buf[idx++] = ((uint8_t*)(&(msg->ackTs)))[1];
    buf[idx++] = ((uint8_t*)(&(msg->ackTs)))[2];
    buf[idx++] = ((uint8_t*)(&(msg->ackTs)))[3];
    return idx;
}
void msgUnpackAck( uint8_t* buf, DDmsgAck* msg ) {
    int idx=1;
    ((uint8_t*)(&(msg->ackTs)))[0] = buf[idx++];
    ((uint8_t*)(&(msg->ackTs)))[1] = buf[idx++];
    ((uint8_t*)(&(msg->ackTs)))[2] = buf[idx++];
    ((uint8_t*)(&(msg->ackTs)))[3] = buf[idx++];
}

#endif // __DD_X_MSG_H

